/**
 * @file : TimeIntegrator.h
 * @author: fenghuwuyu
 * @version 1.0
 * @mail: 3148107006@qq.com
 * @date: 2021年04月19日 星期一 16时08分58秒
 * @brief:
 */

#ifndef TIMEINTEGRATOR_H
#define TIMEINTEGRATOR_H

#include "RHSFactory.h"
#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include <Eigen/LU>
#include <cmath>
#include <Eigen/Core>
#include <algorithm>
#include <Eigen/QR>

class TimeIntegrator
{
	public:
		TimeIntegrator(){};
		~TimeIntegrator(){};
		virtual void Compute() = 0;
};

template <int order>
class AB:public TimeIntegrator
{
	public:
		int p = order; // accuracy 
		AB(); // constructor
		~AB(); // destructor
		// void Set_p(int _p); // change s at the same time
		// void Set_s(int _s);
		// void Set_n(int _n); //In these three functions, remember rebuild the matrix A and b.(A.resize(), b.resize())
		void Set_E(double _E); //set E
		void Set_k(double _k); // set k
		void Set_T(double _T); // set T
		const std::vector<double> Get_beta(); // get beta
		const double Get_error(); // get error
		const double Get_convengence(); // get convengence
		const double Get_n(); // get n
		const int Get_p(); // get p
		const double Get_k();// get k
		const void Print_U(); // print U
		void initial_value(std::vector<double> &_U0); // intial U
		// std::vector<double> func(ProductRHS *tbrhs, std::vector<double> &_U); // set f
		std::vector<double> func(std::vector<double> &_U);
		void Compute(); // compute U, error and rates of convengence 
		const std::vector<std::vector<double>> Get_U();
	private:
		int s; // s+1 is the length of alpha and beta 
		double k; // time-step size
		int n; // num of iteration
		int N; // num of iteration given by user 
		double E; // error given by user
		double e; // compute error
		double r; // rate of convengence
		double T; // the period given by user 
		std::vector<double> beta; // beta 
		std::vector<std::vector<double>> U; // the double dim vector 
};


template <int order>
class AM:public TimeIntegrator
{
	public:
		int p = order; // accuracy 
		AM(); // constructor
		~AM(); // destructor
		// void Set_p(int _p); // change s at the same time
		// void Set_s(int _s);
		// void Set_n(int _n); //In these three functions, remember rebuild the matrix A and b.(A.resize(), b.resize())
		void Set_E(double _E); //set E
		void Set_k(double _k); // set k
		void Set_T(double _T); // set T
		const std::vector<double> Get_beta(); // get beta
		const double Get_error(); // get error
		const double Get_convengence(); // get convengence
		const double Get_n(); // get n
		const void Print_U(); // print U
		void initial_value(std::vector<double> &_U0); // intial U
		const double Get_k(); // get k
		const std::vector<std::vector<double>> Get_U(); // get U
		std::vector<double> func(std::vector<double> &_U); // set f
		void Compute(); // compute U, error and rates of convengence
		std::vector<double> Newton1(std::vector<std::vector<double>> &_U, int n);

	private:
		int s; // s+1 is the length of alpha and beta 
		double k; // time-step size
		int n; // num of iteration
		int N; // num of iteration given by user 
		double E; // error given by user
		double e; // compute error
		double r; // rate of convengence
		double T; // the period given by user 
		std::vector<double> beta; // beta 
		std::vector<std::vector<double>> U; // the double dim vector 
};


class RK:public TimeIntegrator
{
	public: 
		RK(); // constructor
		~RK(); // destructor
		// void Set_p(int _p); // change s at the same time
		// void Set_s(int _s);
		// void Set_n(int _n); //In these three functions, remember rebuild the matrix A and b.(A.resize(), b.resize())
		void Set_E(double _E); //set E
		void Set_k(double _k); // set k
		void Set_T(double _T); // set T
		const double Get_error(); // get error
		const double Get_convengence(); // get convengence
		const double Get_n(); // get n
		const void Print_U(); // print U
		void initial_value(std::vector<double> &_U0); // intial U
		std::vector<double> func(std::vector<double> &_U); // set f
		void Compute(); // compute U, error and rates of convengence  
		const std::vector<std::vector<double>> Get_U(); // get U
		const double Get_k();// get k
	private: 
		double k; // time-step size
		int n; // num of iteration
		int N; // num of iteration given by user 
		double E; // error given by user
		double e; // compute error
		double r; // rate of convengence
		double T; // the period given by user 
		std::vector<double> y1;
		std::vector<double> y2;
		std::vector<double> y3;
		std::vector<double> y4;
		std::vector<std::vector<double>> U; // the double dim vector 
};

template <int order>
class BDF:public TimeIntegrator
{
	public:
		int p = order; // accuracy 
		BDF(); // constructor
		~BDF(); // destructor
		// void Set_p(int _p); // change s at the same time
		// void Set_s(int _s);
		// void Set_n(int _n); //In these three functions, remember rebuild the matrix A and b.(A.resize(), b.resize())
		void Set_E(double _E); //set E
		void Set_k(double _k); // set k
		void Set_T(double _T); // set T
		const std::vector<double> Get_beta(); // get beta
		const double Get_error(); // get error
		const std::vector<double> Get_alpha();
		const double Get_convengence(); // get convengence
		const double Get_n(); // get n
		const void Print_U(); // print U
		const double Get_k(); // get k
		const std::vector<std::vector<double>> Get_U(); // get U
		void initial_value(std::vector<double> &_U0); // intial U
		std::vector<double> func(std::vector<double> &_U); // set f
		void Compute(); // compute U, error and rates of convengence
		std::vector<double> Newton2(std::vector<std::vector<double>> &_U, int n);

	private:
		int s; // s+1 is the length of alpha and beta 
		double k; // time-step size
		int n; // num of iteration
		int N; // num of iteration given by user 
		double E; // error given by user
		double e; // compute error
		double r; // rate of convengence
		double T; // the period given by user 
		std::vector<double> alpha; // alpha 
		std::vector<double> beta; // beta 
		std::vector<std::vector<double>> U; // the double dim vector 
};



double Max(std::vector<double> &_U1, std::vector<double> &_U2);

const double RK::Get_k()
{
	return k;
}

const std::vector<std::vector<double>> RK::Get_U()
{
	return U;
}

template <int order>
const double AB<order>::Get_k()
{
	return k;
}

template <int order>
const int AB<order>::Get_p()
{
	return p;
}

template <int order>
const std::vector<std::vector<double>> AB<order>::Get_U()
{
	return U;
}

template <int order>
AB<order>::AB()
{
	p = order;
	beta.resize(p+1);
	switch (p)
	{
		case 1:
			beta[0] = 1.0;
			beta[1] = 0.0;
			break;

		case 2:
			beta[0] = - 1.0 / 2.0;
			beta[1] = 3.0 / 2.0;
			beta[2] = 0.0;
			break;

		case 3:
			beta[0] = 5.0 / 12.0;
			beta[1] = - 16.0 / 12.0;
			beta[2] = 23.0 / 12.0;
			beta[3] = 0.0;
			break;

		case 4:
			beta[0] = - 9.0 / 24.0;
			beta[1] = 37.0 / 24.0;
			beta[2] = - 59.0 / 24.0;
			beta[3] = 55.0 / 24.0;
			beta[4] = 0.0;
			break;

		default:
			break;
			
	}
	s = p;
	k = 0.1;
	n = 0;
	N = 10000;
	E = 0.001;
	e = 10000;
	r = 1.0;
	T = 10.0;
	U.resize(1);
	U[0].resize(6);
	for(int i=0;i<6;i++)
	{
		U[0][i] = 1;
	}
}

template <int order>
AB<order>::~AB()
{
	beta.clear();
	U.clear();
}

template <int order>
void AB<order>::Set_E(double _E)
{
	E = _E;
}

template <int order>
void AB<order>::Set_k(double _k)
{
	k = _k;
	N = ceil(T / k);
}

template <int order>
void AB<order>::Set_T(double _T)
{
	T = _T;
	N = ceil(T / k);
}

template <int order>
const std::vector<double> AB<order>::Get_beta()
{
	return beta;
}

template <int order>
const double AB<order>::Get_error()
{
	return e;
}

template <int order>
const double AB<order>::Get_convengence()
{
	return r;
}

template <int order>
const double AB<order>::Get_n()
{
	return n;
}

template <int order>
const void AB<order>::Print_U()
{
	for(int j=0;j<=N;j++)
	{
		for(int i=0;i<6;i++)
		{
			std::cout << U[j][i] << " " ;
		}
		std::cout << "" << std::endl;
	}
}

template <int order>
void AB<order>::initial_value(std::vector<double> &_U0)
{
	for(int i=0;i<6;i++)
	{
		U[0][i] = _U0[i] ;
	}
}

template <int order>
std::vector<double> AB<order>::func(std::vector<double> &_U)
{
	const double _mu = 1 / 81.45;
    std::vector<double> du(6);
    du[0] = _U[3];
    du[1] = _U[4];
    du[2] = _U[5];
    double t2 = _U[0] + _mu - 1.0;
    double t3 = _U[1] * _U[1] + _U[2] * _U[2] + t2 * t2;
    double t4 = sqrt(t3 * t3 * t3);
    double t5 = _U[1] * _U[1] + _U[2] * _U[2] + (t2 + 1.0) * (t2 + 1.0);
    double t6 = sqrt(t5 * t5 * t5);
    du[3] = 2.0 * _U[4] + _U[0] - _mu * t2 / t4 - (1.0 - _mu) * (t2 + 1.0) / t6;
    du[4] = -2.0 * _U[3] + _U[1] - _mu * _U[1] / t4 - (1.0 - _mu) * _U[1] / t6;
    du[5] = - _mu * _U[2] / t4 - (1.0 - _mu) * _U[2] / t6;
    return du;
}

/**
template <int order>
std::vector<double> AB<order>::func(ProductRHS *rhs, std::vector<double> &_U)
{
	std::vector<double> tU;
	tU = rhs->RHS(_U);
	return tU;
}
*/

template <int order>
void AB<order>::Compute()
{
	U.resize(2*N);
	for(int i=1;i<2*N;i++)
	{
		U[i].resize(6);
	}
	// Used for computing U1, U2, U3.
	std::vector<double> tmp1(6);
	std::vector<double> tmp2(6);
	std::vector<double> tmp3(6);
	std::vector<double> y1;
	std::vector<double> y2;
	std::vector<double> y3;
	std::vector<double> y4;

	int t = U[0].size();
	switch (p)
	{
		case 1:
			while(k*n <= T)
			{
				//	U[n+p] = U[n+p-1] + k * sum(beta[j] * func(U[n+j])) 		
				for(int i=0;i<t;i++)
				{
					U[n+p][i] = 0;
					for(int j=0;j<=s;j++)
					{
						U[n+p][i] += (1.0 / (p+1.0)) * U[n+p-1][i] + k * beta[j] * func(U[n+j])[i];
					}
				}
				n++;
			}			
			break;

		case 2:
			while(n<1)
			{		
				y1 = func(U[0]); //tbrhs is a instance of ThreeBody_RHS 
				for(int i=0;i<t;i++)
				{
					tmp1[i] = U[n][i] + (k / 2.0) * y1[i];
				}
				y2 = func(tmp1);
				for(int i=0;i<t;i++)
				{
					tmp2[i] = U[n][i] + (k / 2.0) * y2[i];
				}
				y3 = func(tmp2);
				for(int i=0;i<t;i++)
				{
					tmp3[i] = U[n][i] + k * y3[i]; 
				}
				y4 = func(tmp3);
				for(int i=0;i<t;i++)
				{
					U[n+1][i] = U[n][i] + (k / 6.0) * (y1[i] + 2.0 * y2[i] + 2.0 * y3[i] + y4[i]);
				}
				n++;
			}
			n = 0;	
			while(k*n <= T)
			{
				//	U[n+p] = U[n+p-1] + k * sum(beta[j] * func(U[n+j])) 
				for(int i=0;i<t;i++)
				{
					U[n+p][i] = 0;
					for(int j=0;j<=s;j++)
					{
						U[n+p][i] += (1.0 / (p+1.0)) * U[n+p-1][i] + k * beta[j] * func(U[n+j])[i];
					}
				}
				n++;
			}			
			break;
		case 3:
			while(n<2)
			{		
				y1 = func(U[0]); //tbrhs is a instance of ThreeBody_RHS 
				for(int i=0;i<t;i++)
				{
					tmp1[i] = U[n][i] + (k / 2.0) * y1[i];
				}
				y2 = func(tmp1);
				for(int i=0;i<t;i++)
				{
					tmp2[i] = U[n][i] + (k / 2.0) * y2[i];
				}
				y3 = func(tmp2);
				for(int i=0;i<t;i++)
				{
					tmp3[i] = U[n][i] + k * y3[i]; 
				}
				y4 = func(tmp3);
				for(int i=0;i<t;i++)
				{
					U[n+1][i] = U[n][i] + (k / 6.0) * (y1[i] + 2.0 * y2[i] + 2.0 * y3[i] + y4[i]);
				}
				n++;
			}
			n = 0;
			while(k*n <= T)
			{
				//	U[n+p] = U[n+p-1] + k * sum(beta[j] * func(U[n+j])) 
				for(int i=0;i<t;i++)
				{
					U[n+p][i] = 0;
					for(int j=0;j<=s;j++)
					{
						U[n+p][i] += (1 / (p+1)) * U[n+p-1][i] + k * beta[j] * func(U[n+j])[i];
					}
				}
				n++;
			}			
			break;

		case 4:
			while(n<3)
			{		
				y1 = func(U[0]); //tbrhs is a instance of ThreeBody_RHS 
				for(int i=0;i<t;i++)
				{
					tmp1[i] = U[n][i] + (k / 2) * y1[i];
				}
				y2 = func(tmp1);
				for(int i=0;i<t;i++)
				{
					tmp2[i] = U[n][i] + (k / 2) * y2[i];
				}
				y3 = func(tmp2);
				for(int i=0;i<t;i++)
				{
					tmp3[i] = U[n][i] + k * y3[i]; 
				}
				y4 = func(tmp3);
				for(int i=0;i<t;i++)
				{
					U[n+1][i] = U[n][i] + (k / 6) * (y1[i] + 2 * y2[i] + 2 * y3[i] + y4[i]);
				}
				n++;
			}
			n = 0;
			while(k*n <= T)
			{
				//	U[n+p] = U[n+p-1] + k * sum(beta[j] * func(U[n+j])) 
				for(int i=0;i<t;i++)
				{
					U[n+p][i] = 0;
					for(int j=0;j<=s;j++)
					{
						U[n+p][i] += (1 / (p+1)) * U[n+p-1][i] + k * beta[j] * func(U[n+j])[i];
					}
				}
				n++;
			}			
			break;

		default:
			std::cout << "maybe the order is too high" << std::endl;
			break;			
	}
}
template <int order>
const double AM<order>::Get_k()
{
	return k;
}

template <int order>
const std::vector<std::vector<double>> AM<order>::Get_U()
{
	return U;
}

template <int order>
AM<order>::AM()
{
	p = order;
	beta.resize(p);
	switch (p)
	{
		case 2:
			beta[0] = 1.0 / 2.0;
			beta[1] = 1.0 / 2.0;
			break;

		case 3:
			beta[0] = - 1.0 / 12.0;
			beta[1] = 8.0 / 12.0;
			beta[2] = 5.0 / 12.0;
			break;

		case 4:
			beta[0] = 1.0 / 24.0;
			beta[1] = - 5.0 / 24.0;
			beta[2] = 19.0 / 24.0;
			beta[3] = 9.0 / 24.0;
			break;

		case 5:
			beta[0] = - 19.0 / 720.0;
			beta[1] = 106.0 / 720.0;
			beta[2] = - 264.0 / 720.0;
			beta[3] = 646.0 / 720.0;
			beta[4] = 251.0 / 720.0;
			break;

		default:
			break;
			
	}
	s = p-1;
	k = 0.1;
	n = 0;
	E = 0.001;
	e = 10000;
	r = 1;
	T = 10;
	U.resize(1);
	for(int i=0;i<6;i++)
	{
		U[0].push_back(1);
	}
}

template <int order>
AM<order>::~AM()
{
	beta.clear();
	U.clear();
}

template <int order>
void AM<order>::Set_E(double _E)
{
	E = _E;
}

template <int order>
void AM<order>::Set_k(double _k)
{
	k = _k;
	N = ceil(T / k);
}

template <int order>
void AM<order>::Set_T(double _T)
{
	T = _T;
	N = ceil(T / k);
}

template <int order>
const std::vector<double> AM<order>::Get_beta()
{
	return beta;
}

template <int order>
const double AM<order>::Get_error()
{
	return e;
}

template <int order>
const double AM<order>::Get_convengence()
{
	return r;
}

template <int order>
const double AM<order>::Get_n()
{
	return n;
}

template <int order>
const void AM<order>::Print_U()
{
    for(int j=0;j<=N;j++)                
    {                                    
        for(int i=0;i<6;i++)             
        {                                
            std::cout << U[j][i] << " " ;
        }                                
        std::cout << "" << std::endl;    
    }
}

template <int order>
void AM<order>::initial_value(std::vector<double> &_U0)
{
	for(int i=0;i<6;i++)
	{
		U[0][i] = _U0[i];
	}
}

template <int order>
std::vector<double> AM<order>::func(std::vector<double> &_U)
{
    const double _mu = 1 / 81.45;          
    std::vector<double> du(6);             
    du[0] = _U[3];                         
    du[1] = _U[4];                         
    du[2] = _U[5];                         
    double t2 = _U[0] + _mu - 1.0;         
    double t3 = _U[1] * _U[1] + _U[2] * _U[2] + t2 * t2;                              
    double t4 = sqrt(t3 * t3 * t3);        
    double t5 = _U[1] * _U[1] + _U[2] * _U[2] + (t2 + 1.0) * (t2 + 1.0);              
    double t6 = sqrt(t5 * t5 * t5);        
    du[3] = 2.0 * _U[4] + _U[0] - _mu * t2 / t4 - (1.0 - _mu) * (t2 + 1.0) / t6;      
    du[4] = -2.0 * _U[3] + _U[1] - _mu * _U[1] / t4 - (1.0 - _mu) * _U[1] / t6;       
    du[5] = - _mu * _U[2] / t4 - (1.0 - _mu) * _U[2] / t6;                            
    return du;                             
}

template <int order>
std::vector<double> AM<order>::Newton1(std::vector<std::vector<double>> &_U, int n)
{
	const double mu = 1.0 / 81.45;
	double t1 = _U[n+s][1] * _U[n+s][1] + _U[n+s][2] * _U[n+s][2] + (_U[n+s][0] + mu - 1) * (_U[n+s][0] + mu - 1);
	double t2 = _U[n+s][1] * _U[n+s][1] + _U[n+s][2] * _U[n+s][2] + (_U[n+s][0] + mu) * (_U[n+s][0] + mu);
	Eigen::Matrix<double, 6, 6> Jacobi_f; // f'(U[n+s])
	Eigen::Matrix<double, 6, 6> Jacobi_g; // g'(U[n+s])
	Eigen::Matrix<double, 6, 6> I; // I
	Eigen::Matrix<double, 6, 1> U; // _U[n+s]'s matrix 
	Eigen::Matrix<double, 6, 1> U1; // U[n+s-1]'s matrix 
	Eigen::Matrix<double, 6, 1> tf; // used to storage vector f(U[n+j])
	Eigen::Matrix<double, 6, 1> Nu; // new U[n+s], just is U[n+s]_{m} with input U[n+s]_{m-1}, which the function will return
	Eigen::Matrix<double, 6, 1> b; // b
	Eigen::Matrix<double, 6, 1> g; // g
	Eigen::Matrix<double, 6, 1> f; // f
	Eigen::Matrix<double, 6, 1> t; // tmp Matrix
	std::vector<double> F(6); // vector f
	std::vector<double> NU(6); // matrix Nu's vector version, which the function will exactly return 
	t << 0,
		 0,
		 0,
		 0,
		 0,
		 0; // intial t

	F = func(_U[n+s]);

	for(int i=0;i<6;i++)
	{
		f(i, 0) = F[i];
	} // set f

	for(int i=0;i<6;i++)
	{
		U(i, 0) = _U[n+s][i];
	} // intial U

	for(int i=0;i<6;i++)
	{
		U1(i, 0) = _U[n+s-1][i];
	}

	tf << 0,
		 0,
		 0,
		 0,
		 0,
		 0; // intial tf 

	for(int j=0;j<=s;j++)
	{
		F.clear();
		F.resize(6);
		F = func(_U[n+j]);
		for(int i=0;i<6;i++)
		{
			tf(i, 0) += k* beta[j] * F[i];
		}
	}

	g = U - U1 - tf;

	I << 1, 0, 0, 0, 0, 0,
		 0, 1, 0, 0, 0, 0,
		 0, 0, 1, 0, 0, 0,
		 0, 0, 0, 1, 0, 0,
		 0, 0, 0, 0, 1, 0,
		 0, 0, 0, 0, 0, 1; // get I

	Jacobi_f << 0, 0, 0, 1.0, 0, 0,
				0, 0, 0, 0, 1.0, 0,
				0, 0, 0, 0.0, 0, 1.0,
				1.0-mu * pow(t1, -1.5)+3.0*mu*(_U[n+s][0]+mu-1.0)*(_U[n+s][0]+mu-1.0)*pow(t1, -2.5)-(1.0-mu)*pow(t2, -1.5)+3.0*(1.0-mu)*(_U[n+s][0]+mu)*(_U[n+s][0]+mu)*pow(t2, -2.5), 3.0*mu*(_U[n+s][0]+mu-1.0)*pow(t1, -2.5)*_U[n+s][1]+3.0*(1.0-mu)*(_U[n+s][0]+mu)*pow(t2, -2.5)*_U[n+s][1], 3.0*mu*(_U[n+s][0]+mu-1.0)*pow(t1, -2.5)*_U[n+s][2]+3.0*(1.0-mu)*(_U[n+s][0]+mu)*pow(t2, -2.5)*_U[n+s][1], 0.0, 2.0, 0.0,
				3.0*mu*_U[n+s][1]*pow(t1, -2.5)*(_U[n+s][0]+mu-1.0)+3.0*(1-mu)*_U[n+s][1]*pow(t2, -2.5)*(_U[n+s][0]+mu), 1.0-mu*pow(t1, -1.5)+3.0*mu*_U[n+s][1]*pow(t1, -2.5)*_U[n+s][1]-(1.0-mu)*pow(t2, -1.5)+3.0*(1.0-mu)*_U[n+s][1]*pow(t2, -2.5)*_U[n+s][1], 3.0*mu*_U[n+s][1]*pow(t1, -2.5)*_U[n+s][2]+3*(1.0-mu)*_U[n+s][1]*pow(t2, -2.5)*_U[n+s][2], -2.0, 0, 0,
				3.0*mu*_U[n+s][2]*pow(t1, -2.5)*(_U[n+s][0]+mu-1.0)+3.0*(1.0-mu)*_U[n+s][1]*pow(t2, -2.5)*(_U[n+s][0]+mu), 3.0*mu*_U[n+s][2]*pow(t1, -2.5)*_U[n+s][1]+3.0*(1-mu)*_U[n+s][2]*pow(t2, -2.5)*_U[n+s][1], -mu*pow(t1, -1.5)+3.0*mu*_U[n+s][2]*pow(t1, -2.5)*_U[n+s][2]-(1.0-mu)*pow(t2, -1.5)+3.0*(1.0-mu)*_U[n+s][2]*pow(t2, -2.5)*_U[n+s][2], 0, 0, 0;
	Jacobi_g = I - k * beta[s] * Jacobi_f;  // set f'(U) and g'(U) and g'(U) is actually A
		
	 b = Jacobi_g * U - g;

	 Nu = Jacobi_g.fullPivLu().solve(b);

	 for(int i=0;i<6;i++)
	{
		NU[i] = Nu(i, 0);
	}

	return NU;
	
	
}


template <int order>
void AM<order>::Compute()
{
	U.resize(2*N);
	for(int i=1;i<2*N;i++)
	{
		U[i].resize(6);
	}
	int m = 0;
	// Used for computing U1, U2, U3.
	std::vector<double> tmp1(6);
	std::vector<double> tmp2(6);
	std::vector<double> tmp3(6);
	std::vector<double> y1(6);
	std::vector<double> y2(6);
	std::vector<double> y3(6);
	std::vector<double> y4(6);

	std::vector<double> U_1(6); // Used for Newton method.

	int t = U[0].size();
	switch (p)
	{
		case 2:
			while(k*n <= T)
			{
				U[n+s][0] = 1;
				U[n+s][1] = 0;
				U[n+s][2] = 0;
				U[n+s][3] = -1;
				U[n+s][4] = 0;
				U[n+s][5] = 0;
				U_1 = Newton1(U, n);
				while((Max(U[n+s], U_1) > k) & (m < 1000))
				{
					U_1.clear();
					U_1.resize(6);
					U_1 = Newton1(U, n);
					U[n+s].swap(U_1);
					m++;
				}

				m = 0;
				n++;
			}			
			break;

		case 3:
			while(n<1)
			{		
				y1 = func(U[n]); //tbrhs is a instance of ThreeBody_RHS 
				for(int i=0;i<t;i++)
				{
					tmp1[i] = U[n][i] + (k / 2.0) * y1[i];
				}
				y2 = func(tmp1);
				for(int i=0;i<t;i++)
				{
					tmp2[i] = U[n][i] + (k / 2.0) * y2[i];
				}
				y3 = func(tmp2);
				for(int i=0;i<t;i++)
				{
					tmp3[i] = U[n][i] + k * y3[i]; 
				}
				y4 = func(tmp3);
				for(int i=0;i<t;i++)
				{
					U[n+1][i] = U[n][i] + (k / 6.0) * (y1[i] + 2.0 * y2[i] + 2.0 * y3[i] + y4[i]);
				}
				n++;
			}
			tmp1.clear();
			tmp2.clear();
			tmp3.clear();
			y1.clear();
			y2.clear();
			y3.clear();
			y4.clear();
			n = 0;
			while(k*n <= T)
			{
				U[n+s][0] = 1;
				U[n+s][1] = 0;
				U[n+s][2] = 0;
				U[n+s][3] = -1;
				U[n+s][4] = 0;
				U[n+s][5] = 0;
				U_1 = Newton1(U, n);
				while((Max(U[n+s], U_1) > k) & (m < 1000))
				{
					U_1.clear();
					U_1.resize(6);
					U_1 = Newton1(U, n);
					U[n+s].swap(U_1);
					m++;
				}
				m = 0;
				n++;
			}			
			break;

		case 4:
			while(n<2)
			{		
				y1 = func(U[n]); //tbrhs is a instance of ThreeBody_RHS 
				for(int i=0;i<t;i++)
				{
					tmp1[i] = U[n][i] + (k / 2.0) * y1[i];
				}
				y2 = func(tmp1);
				for(int i=0;i<t;i++)
				{
					tmp2[i] = U[n][i] + (k / 2.0) * y2[i];
				}
				y3 = func(tmp2);
				for(int i=0;i<t;i++)
				{
					tmp3[i] = U[n][i] + k * y3[i]; 
				}
				y4 = func(tmp3);
				for(int i=0;i<t;i++)
				{
					U[n+1][i] = U[n][i] + (k / 6.0) * (y1[i] + 2.0 * y2[i] + 2.0 * y3[i] + y4[i]);
				}
				n++;
			}
			tmp1.clear();
			tmp2.clear();
			tmp3.clear();
			y1.clear();
			y2.clear();
			y3.clear();
			y4.clear();
			n = 0;
			while(k*n <= T)
			{
				U[n+s][0] = 1;
				U[n+s][1] = 0;
				U[n+s][2] = 0;
				U[n+s][3] = -1;
				U[n+s][4] = 0;
				U[n+s][5] = 0;
				U_1 = Newton1(U, n);
				while((Max(U[n+s], U_1) > k) & (m < 1000))
				{
					U_1.clear();
					U_1.resize(6);
					U_1 = Newton1(U, n);
					U[n+s].swap(U_1);
					m++;
				}
				m = 0;
				n++;
			}			
			break;
		case 5:
			while(n<3)
			{		
				y1 = func(U[n]); //tbrhs is a instance of ThreeBody_RHS 
				for(int i=0;i<t;i++)
				{
					tmp1[i] = U[n][i] + (k / 2.0) * y1[i];
				}
				y2 = func(tmp1);
				for(int i=0;i<t;i++)
				{
					tmp2[i] = U[n][i] + (k / 2.0) * y2[i];
				}
				y3 = func(tmp2);
				for(int i=0;i<t;i++)
				{
					tmp3[i] = U[n][i] + k * y3[i]; 
				}
				y4 = func(tmp3);
				for(int i=0;i<t;i++)
				{
					U[n+1][i] = U[n][i] + (k / 6.0) * (y1[i] + 2.0 * y2[i] + 2.0 * y3[i] + y4[i]);
				}
				n++;
			}
			tmp1.clear();
			tmp2.clear();
			tmp3.clear();
			y1.clear();
			y2.clear();
			y3.clear();
			y4.clear();
			n = 0;
			while(k*n <= T)
			{
				U[n+s][0] = 1;
				U[n+s][1] = 0;
				U[n+s][2] = 0;
				U[n+s][3] = -1;
				U[n+s][4] = 0;
				U[n+s][5] = 0;
				U_1 = Newton1(U, n);
				while((Max(U[n+s], U_1) > k) & (m < 1000))
				{
					U_1.clear();
					U_1.resize(6);
					U_1 = Newton1(U, n);
					U[n+s].swap(U_1);
					m++;
				}
				m = 0;
				n++;
			}			
			break;

		default:
			std::cout << "maybe the order is too high" << std::endl;
			break;		
	}
 
}




RK::RK()
{
	U.resize(1);
	for(int i=0;i<6;i++)
	{
		U[0].push_back(1);
	}
	k = 0.1;
	n = 0;
	E = 0.001;
	e = 10000;
	r = 1;
	T = 10;
}

RK::~RK()
{
	y1.clear();
	y2.clear();		
	y3.clear();		
	y4.clear();
	U.clear();
}

void RK::Set_E(double _E)
{
	E = _E;
}

void RK::Set_k(double _k)
{
	k = _k;
	N = ceil(T / k);
}

void RK::Set_T(double _T)
{
	T = _T;
	N = ceil(T / k);
}

const double RK::Get_error()
{
	return e;
}

const double RK::Get_convengence()
{
	return r;
}

const double RK::Get_n()
{
	return n;
}

const void RK::Print_U()
{
    for(int j=0;j<=N;j++)                  
    {                                      
        for(int i=0;i<6;i++)               
        {                                  
            std::cout << U[j][i] << " " ;  
        }                                  
        std::cout << "" << std::endl;      
    }                                      
}

void RK::initial_value(std::vector<double> &_U0)
{
	for(int i=0;i<6;i++)
	{
		U[0][i] = _U0[i];
	}
}

std::vector<double> RK::func(std::vector<double> &_U)
{
    const double _mu = 1 / 81.45;          
    std::vector<double> du(6);             
    du[0] = _U[3];                         
    du[1] = _U[4];                         
    du[2] = _U[5];
    double t2 = _U[0] + _mu - 1.0;
    double t3 = _U[1] * _U[1] + _U[2] * _U[
2] + t2 * t2; 
    double t4 = sqrt(t3 * t3 * t3);
    double t5 = _U[1] * _U[1] + _U[2] * _U[
2] + (t2 + 1.0) * (t2 + 1.0);
    double t6 = sqrt(t5 * t5 * t5);
    du[3] = 2.0 * _U[4] + _U[0] - _mu * t2 
/ t4 - (1.0 - _mu) * (t2 + 1.0) / t6;
    du[4] = -2.0 * _U[3] + _U[1] - _mu * _U
[1] / t4 - (1.0 - _mu) * _U[1] / t6;
    du[5] = - _mu * _U[2] / t4 - (1.0 - _mu
) * _U[2] / t6;                            
    return du;                             

}

void RK::Compute()
{
	U.resize(2*N);
	for(int i=1;i<2*N;i++)
	{
		U[i].resize(6);
	}
	std::vector<double> tmp1(6);
	std::vector<double> tmp2(6);
	std::vector<double> tmp3(6);
	int t = U[0].size();

	while(k*n <= T)
	{		
		y1 = func(U[n]); //tbrhs is a instance of ThreeBody_RHS 
		for(int i=0;i<t;i++)
		{
			tmp1[i] = U[n][i] + (k / 2.0) * y1[i];
		}
		y2 = func(tmp1);
		for(int i=0;i<t;i++)
		{
			tmp2[i] = U[n][i] + (k / 2.0) * y2[i];
		}
		y3 = func(tmp2);
		for(int i=0;i<t;i++)
		{
			tmp3[i] = U[n][i] + k * y3[i]; 
		}
		y4 = func(tmp3);
		for(int i=0;i<t;i++)
		{
			U[n+1][i] = U[n][i] + (k / 6.0) * (y1[i] + 2.0 * y2[i] + 2.0 * y3[i] + y4[i]);
		}
		n++;
	}
	tmp1.clear();
	tmp2.clear();
	tmp3.clear();
}

template <int order>
BDF<order>::BDF()
{
	p = order;
	beta.resize(p+1);
	alpha.resize(p+1);
		switch (p)
	{
		case 1:
			alpha[0] = - 1.0;
			alpha[1] = 1.0;
			beta[0] = 0.0;
			beta[1] = 1.0;
			break;

		case 2:
			alpha[0] = 1.0 / 3.0;
			alpha[1] = - 4.0 / 3.0;
			alpha[2] = 1.0;
			beta[0] = 0.0;
			beta[1] = 0.0;
			beta[2] = 2.0 / 3.0;
			break;

		case 3:
			alpha[0] = - 2.0 / 11.0;
			alpha[1] = 9.0 / 11.0;
			alpha[2] = - 18.0 / 11.0;
			alpha[3] = 1.0;
			beta[0] = 0.0;
			beta[1] = 0.0;
			beta[2] = 0.0;
			beta[3] = 6.0 / 11.0;
			break;

		case 4:
			alpha[0] = 3.0 / 25.0;
			alpha[1] = - 16.0 / 25.0;
			alpha[2] = 36.0 / 25.0;
			alpha[3] = - 48.0 / 25.0;
			alpha[4] = 1.0;
			beta[0] = 0.0;
			beta[1] = 0.0;
			beta[2] = 0.0;
			beta[3] = 0.0;
			beta[4] = 12.0 / 25.0;
			break;

		default:
			break;
			
	}
	s = p;
	k = 0.1;
	n = 0;
	E = 0.001;
	e = 10000;
	r = 1;
	T = 10;
	U.resize(1);
	for(int i=0;i<6;i++)
	{
		U[0].push_back(1);
	}
}

template <int order>
BDF<order>::~BDF()
{
	beta.clear();
	alpha.clear();
	U.clear();
}

template <int order>
void BDF<order>::Set_E(double _E)
{
	E = _E;
}

template <int order>
void BDF<order>::Set_k(double _k)
{
	k = _k;
	N = ceil(T / k);
}

template <int order>
void BDF<order>::Set_T(double _T)
{
	T = _T;
	N = ceil(T / k);
}

template <int order>
const std::vector<double> BDF<order>::Get_beta()
{
	return beta;
}

template <int order>
const std::vector<double> BDF<order>::Get_alpha()
{
	return alpha;
}

template <int order>
const double BDF<order>::Get_error()
{
	return e;
}

template <int order>
const double BDF<order>::Get_convengence()
{
	return r;
}

template <int order>
const double BDF<order>::Get_n()
{
	return n;
}

template <int order>
const void BDF<order>::Print_U()
{
    for(int j=0;j<=N;j++)                
    {                                    
        for(int i=0;i<6;i++)             
        {                                
            std::cout << U[j][i] << " " ;
        }                                
        std::cout << "" << std::endl;   
	}
}

template <int order>
void BDF<order>::initial_value(std::vector<double> &_U0)
{
    for(int i=0;i<6;i++)                                                                                 
    {                                                                                                    
        U[0][i] = _U0[i]; 
    }                                                                                                    
}

template <int order>
std::vector<double> BDF<order>::func(std::vector<double> &_U)
{
    const double _mu = 1 / 81.45;                                                                        
    std::vector<double> du(6);                                                                           
    du[0] = _U[3];                                                                                       
    du[1] = _U[4];                                                                                       
    du[2] = _U[5];                                                                                       
    double t2 = _U[0] + _mu - 1.0;                                                                       
    double t3 = _U[1] * _U[1] + _U[2] * _U[2] + t2 * t2;                                                 
    double t4 = sqrt(t3 * t3 * t3);                                                                      
    double t5 = _U[1] * _U[1] + _U[2] * _U[2] + (t2 + 1.0) * (t2 + 1.0);                                 
    double t6 = sqrt(t5 * t5 * t5);                                                                      
    du[3] = 2.0 * _U[4] + _U[0] - _mu * t2 / t4 - (1.0 - _mu) * (t2 + 1.0) / t6;                         
    du[4] = -2.0 * _U[3] + _U[1] - _mu * _U[1] / t4 - (1.0 - _mu) * _U[1] / t6;                          
    du[5] = - _mu * _U[2] / t4 - (1.0 - _mu) * _U[2] / t6;                                               
    return du;                                                                                           
	
}

template <int order>
void BDF<order>::Compute()
{
    U.resize(2*N);                                                                                       
    for(int i=1;i<2*N;i++)                                                                               
    {                                                                                                    
        U[i].resize(6);                                                                                  
    }                                                                                                    
	int m = 0;
	// Used for computing U1, U2, U3.
	std::vector<double> tmp1(6);
	std::vector<double> tmp2(6);
	std::vector<double> tmp3(6);
	std::vector<double> y1(6);
	std::vector<double> y2(6);
	std::vector<double> y3(6);
	std::vector<double> y4(6);

	std::vector<double> U_1(6);

	int t = U[0].size();
	switch (p)
	{
		case 1:
			while(k*n <= T)
			{
				U[n+s][0] = 1;
				U[n+s][1] = 0;
				U[n+s][2] = 0;
				U[n+s][3] = -1;
				U[n+s][4] = 0;
				U[n+s][5] = 0;
				U_1 = Newton2(U, n);
				while((Max(U[n+s], U_1) > k) & (m < 1000))
				{
					U_1.clear();
					U_1.resize(6);
					U_1 = Newton2(U, n);
					U[n+s].swap(U_1);
					m++;
				}
				m=0;
				n++;
			}			
			break;

		case 2:
			while(n<1)
			{		
				y1 = func(U[n]); //tbrhs is a instance of ThreeBody_RHS 
				for(int i=0;i<t;i++)
				{
					tmp1[i] = U[n][i] + (k / 2.0) * y1[i];
				}
				y2 = func(tmp1);
				for(int i=0;i<t;i++)
				{
					tmp2[i] = U[n][i] + (k / 2.0) * y2[i];
				}
				y3 = func(tmp2);
				for(int i=0;i<t;i++)
				{
					tmp3[i] = U[n][i] + k * y3[i]; 
				}
				y4 = func(tmp3);
				for(int i=0;i<t;i++)
				{
					U[n+1][i] = U[n][i] + (k / 6.0) * (y1[i] + 2.0 * y2[i] + 2.0 * y3[i] + y4[i]);
				}
				n++;
			}
			tmp1.clear();
			tmp2.clear();
			tmp3.clear();
			y1.clear();
			y2.clear();
			y3.clear();
			y4.clear();
			n = 0;
			while(k*n <= T)
			{
				U[n+s][0] = 1;
				U[n+s][1] = 0;
				U[n+s][2] = 0;
				U[n+s][3] = -1;
				U[n+s][4] = 0;
				U[n+s][5] = 0;
				U_1 = Newton2(U, n);
				while((Max(U[n+s], U_1) > k) & (m < 1000))
				{
					U_1.clear();
					U_1.resize(6);
					U_1 = Newton2(U, n);
					U[n+s].swap(U_1);
					m++;
				}
				m=0;
				n++;
			}			
			break;
		case 3:
			while(n<2)
			{		
				y1 = func(U[n]); //tbrhs is a instance of ThreeBody_RHS 
				for(int i=0;i<t;i++)
				{
					tmp1[i] = U[n][i] + (k / 2.0) * y1[i];
				}
				y2 = func(tmp1);
				for(int i=0;i<t;i++)
				{
					tmp2[i] = U[n][i] + (k / 2.0) * y2[i];
				}
				y3 = func(tmp2);
				for(int i=0;i<t;i++)
				{
					tmp3[i] = U[n][i] + k * y3[i]; 
				}
				y4 = func(tmp3);
				for(int i=0;i<t;i++)
				{
					U[n+1][i] = U[n][i] + (k / 6.0) * (y1[i] + 2.0 * y2[i] + 2.0 * y3[i] + y4[i]);
				}
				n++;
			}
			tmp1.clear();
			tmp2.clear();
			tmp3.clear();
			y1.clear();
			y2.clear();
			y3.clear();
			y4.clear();
			n = 0;
			while(k*n <= T)
			{
				U[n+s][0] = 1;
				U[n+s][1] = 0;
				U[n+s][2] = 0;
				U[n+s][3] = -1;
				U[n+s][4] = 0;
				U[n+s][5] = 0;
				U_1 = Newton2(U, n);
				while((Max(U[n+s], U_1) > k) & (m < 1000))
				{
					U_1.clear();
					U_1.resize(6);
					U_1 = Newton2(U, n);
					U[n+s].swap(U_1);
					m++;
				}
				m=0;
				n++;

			}			
			break;
		case 4:
			while(n<3)
			{		
				y1 = func(U[n]); //tbrhs is a instance of ThreeBody_RHS 
				for(int i=0;i<t;i++)
				{
					tmp1[i] = U[n][i] + (k / 2.0) * y1[i];
				}
				y2 = func(tmp1);
				for(int i=0;i<t;i++)
				{
					tmp2[i] = U[n][i] + (k / 2.0) * y2[i];
				}
				y3 = func(tmp2);
				for(int i=0;i<t;i++)
				{
					tmp3[i] = U[n][i] + k * y3[i]; 
				}
				y4 = func(tmp3);
				for(int i=0;i<t;i++)
				{
					U[n+1][i] = U[n][i] + (k / 6.0) * (y1[i] + 2.0 * y2[i] + 2.0 * y3[i] + y4[i]);
				}
				n++;
			}
			tmp1.clear();
			tmp2.clear();
			tmp3.clear();
			y1.clear();
			y2.clear();
			y3.clear();
			y4.clear();
			n = 0;
			while(k*n <= T)
			{
				U[n+s][0] = 1;
				U[n+s][1] = 0;
				U[n+s][2] = 0;
				U[n+s][3] = -1;
				U[n+s][4] = 0;
				U[n+s][5] = 0;
				U_1 = Newton2(U, n);
				while((Max(U[n+s], U_1) > k) & (m < 1000))
				{
					U_1.clear();
					U_1.resize(6);
					U_1 = Newton2(U, n);
					U[n+s].swap(U_1);
					m++;
				}
				m=0;
				n++;
			}			
			break;

		default:
			std::cout << "maybe the order is too high" << std::endl;
			break;			
	}

	U_1.clear();
}



template <int order>
std::vector<double> BDF<order>::Newton2(std::vector<std::vector<double>> &_U, int n)
{
	const double mu = 1.0 / 81.45;
	double t1 = _U[n+s][1] * _U[n+s][1] + _U[n+s][2] * _U[n+s][2] + (_U[n+s][0] + mu - 1) * (_U[n+s][0] + mu - 1);
	double t2 = _U[n+s][1] * _U[n+s][1] + _U[n+s][2] * _U[n+s][2] + (_U[n+s][0] + mu) * (_U[n+s][0] + mu);
	Eigen::Matrix<double, 6, 6> Jacobi_f; // f'(U)
	Eigen::Matrix<double, 6, 6> Jacobi_g; // g'(U)
	Eigen::Matrix<double, 6, 6> I; // I
	Eigen::Matrix<double, 6, 1> U; // _U's matrix 
	Eigen::Matrix<double, 6, 1> Nu; // new U, just is U_{m} with input U_{m-1}, which the function will return
	Eigen::Matrix<double, 6, 1> b; // b
	Eigen::Matrix<double, 6, 1> g; // g
	Eigen::Matrix<double, 6, 1> f; // f
	Eigen::Matrix<double, 6, 1> t; // tmp Matrix 
	std::vector<double> F(6); // f vector version 
	std::vector<double> NU(6); // matrix Nu's vector version, which the function will exactly return 
	t << 0,
		 0,
		 0,
		 0,
		 0,
		 0; // intial t

    F = func(_U[n+s]);                   

	for(int i=0;i<6;i++)
	{
		f(i, 0) = F[i];
	} // set f

	for(int i=0;i<s;i++)
	{
		for(int j=0;j<6;j++)
		{
			t(j, 0) += alpha[i] * _U[n+i][j];
		}
	}


	for(int i=0;i<6;i++)
	{
		U(i, 0) = _U[n+s][i];
	} // intial U


	g = k * beta[s] * f - alpha[s] * U - t; 

	I << 1, 0, 0, 0, 0, 0,
		 0, 1, 0, 0, 0, 0,
		 0, 0, 1, 0, 0, 0,
		 0, 0, 0, 1, 0, 0,
		 0, 0, 0, 0, 1, 0,
		 0, 0, 0, 0, 0, 1; // get I

	Jacobi_f << 0, 0, 0, 1, 0, 0,
				0, 0, 0, 0, 1, 0,
				0, 0, 0, 0, 0, 1,
				1.0-mu * pow(t1, -1.5)+3.0*mu*(_U[n+s][0]+mu-1.0)*(_U[n+s][0]+mu-1.0)*pow(t1, -2.5)-(1.0-mu)*pow(t2, -1.5)+3.0*(1-mu)*(_U[n+s][0]+mu)*(_U[n+s][0]+mu)*pow(t2, -2.5), 3.0*mu*(_U[n+s][0]+mu-1.0)*pow(t1, -2.5)*_U[n+s][1]+3.0*(1.0-mu)*(_U[n+s][0]+mu)*pow(t2, -2.5)*_U[n+s][1], 3.0*mu*(_U[n+s][0]+mu-1.0)*pow(t1, -2.5)*_U[n+s][2]+3.0*(1.0-mu)*(_U[n+s][0]+mu)*pow(t2, -2.5)*_U[n+s][1], 0, 2.0, 0,
				3.0*mu*_U[n+s][1]*pow(t1, -2.5)*(_U[n+s][0]+mu-1.0)+3.0*(1.0-mu)*_U[n+s][1]*pow(t2, -2.5)*(_U[n+s][0]+mu), 1.0-mu*pow(t1, -1.5)+3.0*mu*_U[n+s][1]*pow(t1, -2.5)*_U[n+s][1]-(1.0-mu)*pow(t2, -1.5)+3.0*(1.0-mu)*_U[n+s][1]*pow(t2, -2.5)*_U[n+s][1], 3.0*mu*_U[n+s][1]*pow(t1, -2.5)*_U[n+s][2]+3.0*(1-mu)*_U[n+s][1]*pow(t2, -2.5)*_U[n+s][2], -2.0, 0, 0,
				3.0*mu*_U[n+s][2]*pow(t1, -2.5)*(_U[n+s][0]+mu-1.0)+3.0*(1.0-mu)*_U[n+s][1]*pow(t2, -2.5)*(_U[n+s][0]+mu), 3.0*mu*_U[n+s][2]*pow(t1, -2.5)*_U[n+s][1]+3.0*(1.0-mu)*_U[n+s][2]*pow(t2, -2.5)*_U[n+s][1], -mu*pow(t1, -1.5)+3.0*mu*_U[n+s][2]*pow(t1, -2.5)*_U[n+s][2]-(1.0-mu)*pow(t2, -1.5)+3.0*(1-mu)*_U[n+s][2]*pow(t2, -2.5)*_U[n+s][2], 0, 0, 0;

	Jacobi_g = k * beta[s] * Jacobi_f - alpha[s] * I;  // set f'(U) and g'(U) and g'(U) is actually A
		
	b = Jacobi_g * U - g;

	Nu = Jacobi_g.lu().solve(b);
	for(int i=0;i<6;i++)
	{
		NU[i] = Nu(i, 0);
	}
	return NU;
	
	
}


double Max(std::vector<double> &_U1, std::vector<double> &_U2)
{
	int l1 = _U1.size();
	int l2 = _U2.size();
	double m = 0;
	if(l1 != l2)
	{
		std::cout << "Max function has error." << std::endl;
	}
	else
	{
		m = abs(_U1[0] - _U2[0]);
		if(abs(_U1[1]-_U2[1]) <= m)
		{
			// DO NOTHING
		}
		else
		{
			m = abs(_U1[1]-_U2[1]);
		}
	}
	return m;
}

template <int order>
const double BDF<order>::Get_k()
{
	return k;
}

template <int order>
const std::vector<std::vector<double>> BDF<order>::Get_U()
{
	return U;
}






#else
// DO THING
#endif

